

#define BOOST_TEST_MODULE MMM_LEGACY_MOTION_READER_XML_TEST

#include <MMM/Model/Model.h>
#include <MMM/Motion/Legacy/LegacyMotion.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
#include <string>

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

BOOST_AUTO_TEST_CASE(testParseLoadMotion)
{
	const std::string motionString = 
		"<?xml version='1.0' encoding='UTF-8'?>"
		"<MMM>"
		"	<Motion name='test'>"
		"		<comments>"
		"			<text>Line1</text>"
		"			<text>Line2</text>"
		"		</comments>"
		"		<jointOrder>"
		"			<joint name='j1'/>"
		"			<joint name='j2'/>"
		"		</jointOrder>"
		"		<MotionFrames>"
		"			<motionFrame>"
		"				<timestep>0</timestep>"
		"				<rootPosition>1 2 3</RootPosition>"
		"				<rootPositionVelocity>4 5 6</rootPositionVelocity>"
		"				<rootPositionAcceleration>7 8 9</rootPositionAcceleration>"
		"				<rootRotation>10 11 12</rootRotation>"
		"				<rootRotationVelocity>13 14 15</rootRotationVelocity>"
		"				<rootRotationAcceleration>16 17 18</rootRotationAcceleration>"
		"				<jointPosition>20 21</jointPosition>"
		"				<jointVelocity>22 23</jointVelocity>"
		"				<jointAcceleration>24 25</jointAcceleration>"
		"			</motionFrame>"
		"		</MotionFrames>"
		"	</Motion>"
		"</MMM>";

    MMM::LegacyMotionReaderXMLPtr r(new MMM::LegacyMotionReaderXML());
	MMM::LegacyMotionPtr m = r->createMotionFromString(motionString,"test");
	BOOST_REQUIRE(m);
	BOOST_REQUIRE(m->getNumFrames()==1);
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootPos().isApprox(Eigen::Vector3f(1.0f,2.0f,3.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootPosVel().isApprox(Eigen::Vector3f(4.0f,5.0f,6.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootPosAcc().isApprox(Eigen::Vector3f(7.0f,8.0f,9.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootRot().isApprox(Eigen::Vector3f(10.0f,11.0f,12.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootRotVel().isApprox(Eigen::Vector3f(13.0f,14.0f,15.0f)));
	BOOST_REQUIRE(m->getMotionFrame(0)->getRootRotAcc().isApprox(Eigen::Vector3f(16.0f,17.0f,18.0f)));
	BOOST_REQUIRE(m->getJointNames().size() == 2);
	BOOST_REQUIRE(m->getJointNames()[0] == "j1");
	BOOST_REQUIRE(m->getJointNames()[1] == "j2");
	BOOST_REQUIRE(m->getName() == "test");


	// check export and re-load data
	std::string motionString2 = m->toXML();
	//std::cout << motionString2 << std::endl;
	std::string start = "<MMM>";
	std::string end = "</MMM>";
	motionString2 = start + motionString2;
	motionString2 = motionString2 + end;
	MMM::LegacyMotionPtr m2 = r->createMotionFromString(motionString2);

	BOOST_REQUIRE(m2);
	BOOST_REQUIRE(m2->getNumFrames()==1);
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootPos().isApprox(Eigen::Vector3f(1.0f,2.0f,3.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootPosVel().isApprox(Eigen::Vector3f(4.0f,5.0f,6.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootPosAcc().isApprox(Eigen::Vector3f(7.0f,8.0f,9.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootRot().isApprox(Eigen::Vector3f(10.0f,11.0f,12.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootRotVel().isApprox(Eigen::Vector3f(13.0f,14.0f,15.0f)));
	BOOST_REQUIRE(m2->getMotionFrame(0)->getRootRotAcc().isApprox(Eigen::Vector3f(16.0f,17.0f,18.0f)));
	BOOST_REQUIRE(m2->getJointNames().size() == 2);
	BOOST_REQUIRE(m2->getJointNames()[0] == "j1");
	BOOST_REQUIRE(m2->getJointNames()[1] == "j2");
}


// BOOST_AUTO_TEST_SUITE_END()
